#include "tic_crow/tic_crow.h"
#include "version.h"

ros::Publisher robot_pose_pub;
void transformPoint(const tf::TransformListener &listener) {
    geometry_msgs::PoseWithCovarianceStamped msg_robotpose;
    try{
        tf::StampedTransform transform;
        ros::Time now = ros::Time::now();
        listener.waitForTransform("map", "base_link", now, ros::Duration(0.2));
        listener.lookupTransform("map", "base_link", now, transform);
        tf::Vector3 origin = transform.getOrigin();
        tf::Quaternion q = transform.getRotation();
        msg_robotpose.header.frame_id = "map";
        msg_robotpose.header.stamp = ros::Time::now();
        msg_robotpose.pose.pose.position.x = origin.x();
        msg_robotpose.pose.pose.position.y = origin.y();
        msg_robotpose.pose.pose.position.z = 0;
        msg_robotpose.pose.pose.orientation.x = 0;
        msg_robotpose.pose.pose.orientation.y = 0;
        msg_robotpose.pose.pose.orientation.z = q[2];
        msg_robotpose.pose.pose.orientation.w = q[3];

        robot_pose_pub.publish(msg_robotpose);
        //ROS_INFO("Origin:(%.2f, %.2f, %.2f)",origin.x(), origin.y(), origin.z());
    }
    catch(tf::TransformException& ex) {
        // ROS_INFO("Received an exception trying to transform from \"map\" ot \"base_footprint\":%s",ex.what());
    }
}

int main(int argc, char** argv)
{
    version_output(argc ,argv);
	ros::init(argc, argv, "tic_crow");
    ros::NodeHandle n;
    tf::TransformListener listener(ros::Duration(0.2));
    robot_pose_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("robot_pose",1000);
    ros::Timer timer = n.createTimer(ros::Duration(0.2) ,boost::bind(&transformPoint ,boost::ref(listener)));

	std::this_thread::sleep_for (std::chrono::seconds(1));
	std::thread t([&](){
        try{
            ros::Rate r(5); 
            while (ros::ok())
            {
                ros::spin();
                r.sleep();
            }
        }
		catch(...){
            t.join();
            throw;
        }
	});   

	tic_crow::AppCrow tic_crow;
    tic_crow.tic_crow();	

    t.join();
    return 0;   
}
